#import serial
#import math
#import struct
import time
from threading import Thread, Lock, Event
from traceback import print_exc

from Brain.vector import Vector
from Brain.io.core import Comm
from Brain.operations.module import Module 

from encoder_calculation import calculate, ISR


class Position:
	def __init__(self):
		self.x = 0.0
		self.y = 0.0
		self.angle = 0.0
		self.velocity_x = 0.0
		self.velocity_y = 0.0
		self.calculation_timestamp = 0.0

class CommunicationData:
	def __init__(self, history):
		self.chunk = ''
		self.history = history

class EncoderResult:
	def __init__(self, pos, angle, velocity):
		self.pos = pos
		self.angle = angle
		self.velocity = velocity
	
	def __str__(self):
		return 'pos %s | angle %s deg' % (self.pos, self.angle * 180.0 / 3.1415, )
	def copy(self):
		return EncoderResult(self.pos.copy(), self.angle, self.velocity.copy())
	def __eq__(self, other):
		if other == None: return False
		return self.pos.x == other.pos.x and self.pos.y == other.pos.y and self.angle == other.angle 
	def __ne__(self, other):
		return self.pos.x != other.pos.x and self.pos.y != other.pos.y and self.angle != other.angle 

class Encoder(Thread, Module):
	""" Encoder module
	"""
	
	AXIAL_DISTANCE = 270.0
	AXIAL_DISTANCE_HALF = 135.0
	PRESKALER = 0
	
	def __init__(self):
		Thread.__init__(self)
		Module.__init__(self)
		self._left = 0
		self._right = 0
		self._maneuver_type = None
		
		## position
		self.position = Position()
		## low level communication, sensor log ..
		self._commdata = CommunicationData([])
		self.history_resource = Lock()
		self.event_sensor_read = Event()

		self.pipe = Comm.link('encoders', self._isr)
		self._result_resource = Lock()
		self._process_index = 0
		
		## time / counters
		self._sample_count = 0
		
		self.new_data_event = Event()

	def _isr(self, new_chunk, time_stamp):
		self.history_resource.acquire()
		ISR( self._commdata, new_chunk, time_stamp )
		self.history_resource.release()
		self.event_sensor_read.set()

	def reset(self, pos, angle):
		""" angle of direction vector """
		self._result_resource.acquire()
		self.position.x, self.position.y, self.position.angle, = pos.x, pos.y, float(angle),
		self._result_resource.release()
	
	def new_maneuver(self, type):
		""" specify different movement type """
		self._maneuver_type = type
	
	def read(self):
		""" read sensor data -> EncoderResult
		"""
		self._result_resource.acquire()
		pos = Vector( self.position.x, self.position.y )
		vel = Vector( self.position.velocity_x, self.position.velocity_y )
		angle = self.position.angle
		self._result_resource.release()
		return EncoderResult(pos, angle, vel)
	
	def _process_input(self, record):
		self._left += record.left
		self._right += record.right
		self._sample_count += 1
		if self._sample_count > self.PRESKALER:
			self._sample_count = 0
			
			## do position calculation
			left, right, = self._left, self._right,
			self._left, self._right = 0.0, 0.0,
			self._result_resource.acquire()
			calculate( self.position, left, right, record.ts )
			self._result_resource.release()	
	
	def run(self):
		while self._working:
			self.event_sensor_read.wait()	## sleep until signaled ..
			self.event_sensor_read.clear()
			while 1:
				self.history_resource.acquire()
				if len(self._commdata.history) > self._process_index:
					record = self._commdata.history[self._process_index]
					self.history_resource.release()
					try:
						self._process_input(record)
						self.new_data_event.set()
					except:
						print_exc()
					self._process_index += 1
				else:
					self.history_resource.release()
					break
	
	def dump_logs(self):
		now = time.strftime('%m-%d-%H%M')
		f = open('encoder_%s.log' % (now, ), 'wb')
		for record in self._commdata.history[:]:
			f.write( 'ts: %.2f\t\tl: %s r: %s\n' % (record.ts, record.left, record.right, ) )
		f.close()

global WORKING
WORKING = True
if __name__ == '__main__':
	
	def KeyBreak():
		global WORKING
		data = raw_input()
		WORKING = False
	
	t = Thread(target = KeyBreak)
	t.setDaemon(True)
	t.start()
	
	encoder = Encoder()
	encoder.setDaemon(True)
	
	print 'position calculation test'
	encoder.start()
	
	while WORKING:
		time.sleep(0.2)
		print encoder.read()









